/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "stdafx.h"
#include "S_PhysicsChrono.h"

#include "mat/Mat44.h"
#include "vec/quat.h"

#include "../A_核心/S_Chrono核心.h"
#include "../B_物理体/Chrono刚体.h"


S_Chrono::S_Chrono() {
	m_Core.m_PhysicalSystem->SetParallelThreadNumber(32);
}

S_Chrono::~S_Chrono() {

}

void S_Chrono::f_Update() {
	m_Core.m_PhysicalSystem->DoStepDynamics(m_步进时间);
	//m_Core.m_PhysicalSystem->DoFrameDynamics(m_步进时间);
}

void S_Chrono::f_应用物理变换() {

}

void S_Chrono::f_取更新物体(std::vector<Mat4X4f>& mat, std::vector<void*>& userData)
{
	auto& objs = m_Core.m_PhysicalSystem->Get_bodylist();
	uint16 num = objs.size();
	auto* world = objs.data();



	userData.resize(mat.size() + num);
	mat.resize(mat.size() + num);

	uint32 begin = 0;
	//print positions of all objects
	for (uint32 i = 0; i < num; ++i) {

		auto* p_Objs = userData.data();
		auto* p_Mat = mat.data();


		auto& coord = objs[i]->GetCoord();
		auto& r = coord.rot.Q_to_Rotv();
		auto& l = coord.pos;
		//f_欧拉角转四元数()
		vec3 loc = { float32(l.x()), float32(l.y()), float32(l.z()) };
		vec3 rot = { float32(r.x()), float32(r.y()), float32(r.z()) };
		auto mat = f_从变换构建矩阵(loc, rot, { 1.0f, 1.0f, 1.0f });
		p_Mat[i] = *((Mat4X4f*)((float32*)mat.columns));
		
		//p_Objs[i] = body->getUserPointer();
		//p_Objs[i]
		//begin = mat.size();
	}
}

void S_Chrono::f_添加物理体(S_物理体* body, uint32 scenekey) {
	switch (body->m_Type) {
	case E_物理体类型::E_物理体_刚体:
		//my_system.AddBody(my_body_A);
		m_Core.m_PhysicalSystem->AddBody(*((std::shared_ptr<chrono::ChBody>*)(body->f_getBody())));
		break;
	case E_物理体类型::E_物理体_柔体:
		break;
	case E_物理体类型::E_物理体_布料:
		break;
	case E_物理体类型::E_物理体_静态:
		m_Core.m_PhysicalSystem->AddBody(*((std::shared_ptr<chrono::ChBody>*)(body->f_getBody())));
		break;
	case E_物理体类型::E_物理体_控制:

		break;
	}
}

void S_Chrono::f_移除物理体(S_物理体* body, uint32 scenekey) {
	//auto& objs = m_Core.m_PhysicalSystem->Get_bodylist();
	auto bodylist = m_Core.m_PhysicalSystem->Get_bodylist();
	

	if (bodylist.size()) {
		auto b = *((std::shared_ptr<chrono::ChBody>*)(body->f_getBody()));
		auto itr = std::find(std::begin(bodylist), std::end(bodylist), b);
		if (itr != bodylist.end()) {
			m_Core.m_PhysicalSystem->RemoveBody(b);
		}
	}

	//m_Core.m_PhysicalSystem.re
}










